
#include <stdio.h>
#include <unistd.h>
/*#include "kalman.h"*/

#define AX_PIN 0
#define AY_PIN 1
#define AZ_PIN 2
#define GZ_PIN 3
#define GX_PIN 4
#define GY_PIN 5

//static const float dt = ( 1024.0 * 256.0 ) / 8000000.0;

#define GRAVITY 101
#define ACCEL_SCALE(x) x*(9.81/GRAVITY)			// Scale to m/s^2
#define GYRO_GAIN 2.5
#define GYRO_SCALE(x) x*((GYRO_GAIN*PI)/360)	// Scale to radians/s

typedef struct {
  float ax, ay, az, gz, gx, gy;	
} data_t;

data_t data;
data_t offsets;

/*
void read_imu(data_t * data) {
	data.ax = analogRead(AX_PIN);
	data.ay = analogRead(AY_PIN);
	data.az = analogRead(AZ_PIN);
	data.gz = analogRead(GZ_PIN);
	data.gx = analogRead(GX_PIN);
	data.gy = analogRead(GY_PIN);	
}
*/

/* Read imu data from file */
void read_imu(FILE * file, data_t * data) {
	fscanf(file, "%f\t%f\t%f\t%f\t%f\t%f\n", 
		&data->gx, &data->gy, &data->gz, &data->ax, &data->ay, &data->az);
}

void avg(data_t * data, data_t * new) {
	data->ax = ((new->ax*.9)+(data->ax*.1));
	data->ay = ((new->ay*.9)+(data->ay*.1));
	data->az = ((new->az*.9)+(data->az*.1));
	data->gz = ((new->gz*.9)+(data->gz*.1));
	data->gx = ((new->gx*.9)+(data->gx*.1));
	data->gy = ((new->gy*.9)+(data->gy*.1));
}

void add_offsets(data_t * data) {
	data->ax -= offsets.ax;
	data->ay -= offsets.ay;
	data->az -= offsets.az;
	data->gx -= offsets.gx;	
	data->gy -= offsets.gy;
	data->gz -= offsets.gz;
}

int main(int argc, char ** argv) 
{
	int matched;
	data_t new;
/*	KALDATA kaldata;*/

	if(argc < 2) {
		printf("esempio: ./prova nome_file_dati_della_imu\n");
		return;
	}
	
/*	kalmanInitState(kaldata);*/
	FILE * file = fopen(argv[1], "r");
	if(file==NULL) {
		perror("Impossibile aprire il file");
		return;
	}
	
	read_imu(file, &offsets);	/* First values read are used as offset	*/
	offsets.az += GRAVITY;		/* Add 101 (raw value of g) to z acc. offset */
	
	do {
		read_imu(file, &new);
		add_offsets(&new);
		avg(&data, &new);
		
/*		kalman_update(data.gx, kaldata, dt);*/
/*		kalman_update(accelerometer_angle, kaldata);*/
	
//		printf("IN \t%f\t%f\t%f\t%f\t%f\t%f\t=>\t%f\n", 
			//new.gx, new.gy, new.gz, new.ax, new.ay, ACCEL_SCALE(new.az), .0);
//		printf("AVG \t%f\t%f\t%f\t%f\t%f\t%f\t=>\t%f\n\n", 
//			data.gx, data.gy, data.gz, data.ax, data.ay, ACCEL_SCALE(data.az), .0);	

		printf("%.2f\t%.2f\t%.2f\t%.2f\t%.2f\t%.2f\n",data.gx, data.gy, data.gz, data.ax, data.ay, ACCEL_SCALE(data.az));	
		usleep(400000);
	} 
	while(1);
}
